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SIX DEGREE OF FREEDOM POSITION RANGING 
GOVERNMENT CONTRACT 

[0001] This application was developed under U.S. Contract MDA972-98-C-0009 in 
conjunction with DARPA. Therefore, the U.S. Government may have certain rights to 
the assignment of this application or any patent issuing therefrom. 

BACKGROUND OF THE INVENTION 

[0002] Embodiments of the present invention are directed to position sensing and 
movement between components, and more particularly to the alignment and/or docking 
between these components. 

[0003] The ability to determine a relative relationship between distinct components 
has considerable uses. For example, offset sensing between two plates is of value for 
automatic active docking of self-configurable robot systems. One such system is known 
as a PolyBot (a robotic system of Xerox Corporation), which uses repeated modules with 
identical docking mechanisms or interface plates. 

[0004] In the past, expensive optics and cameras have been used for docking of 
kinematic chains and robotic arms. One such system is a virtual reality (VR) hardware 
system, which uses six degree of freedom trackers. Such systems include linkage-based 
concepts, electro-magnetic fuel based systems, ultrasonic rangings, and inertial tracking 
methods, among others. The ultrasonic and inertial tracking methods have not been 
extended to six degree of freedom (DOF) in a robust fashion. The vision based methods 
tend to be computationally intensive and expensive. The electro-magnetic based methods 
are the most popular for VR. However, they do not work well for self-configurable 
systems since they are prone to interference of metallic objects and are expensive to 
implement. 

[0005] Other non-light based positional methods include eddy-current sensing, hall 
effect sensors or capacitance based methods. Drawbacks of these methods are the 
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intimate presence of electric motors, which may cause too much noise to make the 
sensing feasible. 

SUMMARY OF THE INVENTION 

[0006] An alignment system includes a first module wherein a plurality of emitters 
are located at defined locations on the face of the first module. The emitters are 
positioned to emit signals of a known intensity distribution away from the face of the 
first module. A first receiver configuration is also located on the face of the first module, 
where the first receiver configuration has a known sensitivity distribution. A second 
module has a second plurality of emitters located at defined locations on the face of the 
module. The second plurality of emitters are positioned to emit signals of a known 
intensity distribution away from the face of the second module. A second receiver 
configuration is also located on the face of the second module, and has a known 
sensitivity distribution. First and second trigger signal generators are configured to fire 
the first and second plurality of emitters, respectively, in predetermined patterns. The 
generated signals are sensed by at least some of the receivers comprising the first and 
second receiver configurations. A converter arrangement is operationally connected to 
the first and second receiver configurations in order to obtain and convert the received 
signals into data representative of the readings received by selected receivers. A 
processing system is in operational connection to the first converter and the second 
converter, and computes at least one of an absolute six degree offset or a relative six 
degree offset between the faces. The offset information is then used to achieve a desired 
alignment between the face of the first module and the face of the second module. The 
operational connection of the components for the system may be accomplished via both 
physical wire type connections as well as through wireless connections. 

BRIEF DESCRIPTION OF THE DRAWINGS 

[0007] The invention may take form in certain parts and in certain steps and 
arrangements of parts and steps, the preferred embodiments of which will be described 
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in detail in this specification and illustrated in the accompanying drawings, which form 
a part herein and wherein: 

[0008] FIGURE 1 is an illustration of a multi-modular robot system, in a linear 
configuration which may implement embodiments of the present invention; 
[0009] FIGURE 2 is the modular robot of FIGURE 1 shown in a different 
configuration; 

[0010] FIGURE 3 illustrates face plates of modules illustrated in FIGURE 2; 
[001 1] FIGURE 4 sets forth an emitting and receiving signal sequence pattern for 
operation of one embodiment of the present invention; 

[0012] FIGURES 5a and 5b set forth actual intensity changes at emitter and receiver 

angles and at various distances between the emitter and the receiver; 

[0013] FIGURES 6a and 6b depict intensity changes of the emitter and receiver 

angles at various distances between them computed by a model; 

[0014] FIGURE 7 depicts a first and second plate and sensor arrangement in a first 

embodiment; 

[001 5] FIGURES 8 shows additional embodiments of the face plates which may be 
used in connection with the modules of FIGURE 2; 

[0016] FIGURE 9 discloses actual and model intensity curves for the second 
embodiment; 

[0017] FIGURE 10 discloses a first and second plate and sensor arrangement in a 
second embodiment; and 

[0018] FIGURE 11 sets forth a block diagram illustrating a system which may 
implement concepts of the present invention; 

DETAILED DESCRIPTION OF PREFERRED EMBODIMENTS 
1. Introduction 

[0019] A simplified view of a modular, self-configurable robot 10 which may 
implement concepts described herein is illustrated in FIGURE 1 . Modular robot 10 is an 
automatically reconfiguring, hardware system that tends to be more homogenous than 
heterogeneous. Particularly, the system may have different types of modules, but the 
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ratio of the number of module types to the total number of modules is low. Systems with 
these characteristics are called n-modular where n refers to the number of module types 
and n is typically 1 or 2. In this example, there may be a main or node module 12, and 
secondary or segment modules 14 and including modules 14a and 14n are substantially 
identical in design. 

[0020] In one embodiment, each module of modular robot 10 may include an 
embedded processor such as a Motorola PowerPC 555 processor, having sufficient 
memory such as 1 megabyte of external RAM. The modules communicate over a semi- 
global bus using, for example, a controller area network (CANbus) standard. 
[002 1 ] Modular reconfigurable robots with multiple modules have the ability to form 
a variety of shapes with a large number of degrees or dimensions of movement. The 
robot may change its shape to suit different tasks. For example, robot 10 shown in 
FIGURE 1 may move to connect its end modules 14a, 14n into a docked arrangement. 
Modular robot 10 can repositioned itself into a substantially different topological design 
like a loop. However, in order to complete this positioning, it is necessary for face plates 
16a and 16n of the corresponding modules 14a and 14n to enter into an aligned position 
as shown in Figure 2 which will permit complete docking and connection between 
modules 14a and 14n. Complete docking may also include the modules having 
mechanical and electrical module connectors. 

[0022] Due to various factors, including mechanical friction open loop errors exist 
which lower the accuracy of the alignment. This accuracy of alignment is a significant 
problem in such docking environments. Therefore, the following discussion will focus 
on an alignment system which improves the accuracy of alignment for docking of the 
robotic modules described. However, it is to be appreciated such an alignment system 
will have other uses in fields different from robotics. 
2. Introduction to a First Face Plate Embodiment 

[0023] Illustrated in FIGURE 3 is a first embodiment of face plates 1 8a, 1 8n which 
may be used for the face plates 16a, 16n of FIGURES 1 and 2. 

[0024] One embodiment of the six degrees or dimensions of freedom position 
ranging system of the present invention implements commercial-off-the-shelf (COTS) 
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infrared (IR) light emitting diode (LED) emitters and COTS IR receivers, on face plates 
18a, 18n. The intensity distribution of the emitters and the sensitivity distribution of the 
receivers allow for estimation of the angle and distance between an opposing plate. The 
designs and operation of the present system, as confirmed by experiments, indicate that 
such a design is capable of providing positional offset information in any of six degrees 
of error (x, y, z, pitch, roll, and yaw) within a prescribed range. 

[0025] The commercial off-the-shelf IR LED emitters and IR receivers were chosen 
for the system for their low cost, small size minimal interface requirements and low 
processing overhead. However, specialized emitters and receivers using infrared or other 
signal transmission and/or reception medians, may also be used in the system for specific 
implementations. 

[0026] Returning to FIGURE 3, opposing plates 18a and 18n are identically 
configured. Four IR LEDs emitters 20a-20d are configured in the corners of face plate 
1 8a. Additionally two IR receivers 22a, 22b are located at the midpoint (or mid-line) and 
substantially near center on face plate 1 8a. Face plate 1 8n includes identically positioned 
LEDs 30a-30d and IR receivers 32a, 32b, positioned at substantially identical locations 
as those on face plate 1 8a. It is to be appreciated, the sensing method and alignment 
capabilities described below may extend to systems which have mechanical and/or 
electrical connection mechanisms, as well as systems where components do not come 
into physical contact but rather are simply aligned. 

[0027] In one embodiment, each of the eight emitters 20a-20d and 30a-30d is lit in 
sequence and an analog reading is taken by receivers 22a, 22b and 32a, 32b. The 
intensity distribution of an emitter and the sensitivity of a receiver permits obtaining six 
degree offset between two opposing plates. Experiments have been conducted which 
provide positional offset in any of six degrees of error (x, y, z, pitch, roll, and yaw) within 
a prescribed range, while the following discussion focuses on a system which provides 
positional offset information related to six degrees of error, it is to be understood the 
concepts may also be applied to systems having more or less degrees of movement. 
[0028] The design ensures that each receiver detects and samples the intensity from 
each emitter on the opposite plate at a distinct time. In order to accomplish this, each of 
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the emitters is lit in sequence and a reading is taken from the opposing receivers. 
Readings are also taken in between the times when the IR LED emitters are active in 
order to measure the ambient IR. 

[0029] FIGURE 4 depicts control signals for one embodiment of the operation of the 
system in accordance with the face plates of FIGURE 3. Particularly, shown are the 
control signals for the first four emitters to be lit. The system may be programmed to 
select a first plate to emit signals during "Time slot 1." The two plates are synchronized 
so that receivers on the plate measure at the correct time. At the end of a time period, 
each of the receivers (assuming four total receivers, two on each plate) will have four 
readings from their opposing emitters, and four ambient readings totaling 32 
measurements. The ambient IR readings are subtracted from the sample in order to 
improve the robustness of the system. Thus, following this subtraction step, the system 
will end up with 16 pieces of data. 

[0030] With continuing attention to FIGURE 4, a triggering or clock signal 50 
generated by trigger signal generators permit the synchronization and operation of the 
data acquisition and sampling sequences. Upon the start of second clock cycle 52, 
emitter 1 becomes active 54. At this time, IR receivers on the plates (the first emitters 
through fourth emitters are sample designations and may be representative of any of the 
emitters previously described) 56 detect the output of emitter 1 . As shown on the next 
clock signal 58, detection at the ambient IR sample point 60 is undertaken. The 
foregoing process is continued for each of the emitters (emitters 2-4) on the first face 
plate. Thereafter in a second time slot of the first period 62 activation and detection of 
the emitters on a second face plate are undertaken. It is to be appreciated while this 
embodiment discloses a particular number of emitters and receivers at specific locations, 
the concepts of the application may be accomplished using other numbers and locations 
for the components. 

3. IR Intensity Model Design of the First Embodiment 

[0031] The concept behind the present system design is based on an idea that the 
intensity detected by a receiver is a function of the distance and/or angle between the 
emitter and receiver, i.e., I = f(e, r, d) where / is the intensity reading, e and r are emitter 
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and receiver angles, respectively, and d is the distance between the emitter and the 
receiver. A model can then be obtained by model fitting for given emitters and detectors. 
[0032] The model of this embodiment was constructed by decomposing /into three 
functions, fe(e),frfr),fd(d) ^ by letting I = A/ e (e) f r (r)/d(d) where A is a scale factor. 
Two separate data collections were undertaken, one fixed r to 0 degrees and changed e 
from 0° to 90° in 5 degree increments, and d was moved from 0.5 inches to 5 inches in 
0.5 inch increments. The other data collection fixed e to 0 and changed r from 0° to 90° 
in 5 degree increments, and d from 0.5 to 5 inches. The results of this data collection are 
plotted in FIGURES 5a and 5b, respectively. FIGURES 5a and 5b therefore show actual 
emitter angle changes where the angles are plotted against intensity at various distances. 
[0033] The following function: 

f e = (A - 0)e-<°--W°- +0 
where A is 1 000, O is 150 and 6° is 20 degrees, is used to fit the data in a least 
squares fashion, and obtain the parameter 0 e as 0.2660. Similarly, function: 

f r = (A - 0> _MW +0 
with 9° r as 40 degrees is used to fit the receiver data, and obtain the parameter oy as 
0.3694. As a result, the IR intensity model is obtained: 

/ = (a - o) t -«.-*t*. A + O . 

Using this model, the corresponding model data is plotted in Figures 6a and 6b. 

Compared with Figures 5a and 5b, the model fits the data relatively well. 

4. Six Dimensional Offset Estimation Methods for the First Embodiment 

[0034] For each plate, attached is a frame as shown in Figure 7 (in this case Plate 1 
(90) and Plate2 (92) are facing each other). Given an offset between the two plates, 
the spatial relationship between each emitter and receiver pair is determined. 
[0035] In this description d is the distance from the receiver to the center of the 
plate, and w and h are the width and height of the position of the emitters. The 
coordinate of receiver 1 in its own frame is <0, 0, d>, and the coordinate of the 
receiver 2 is <0, 0, -d>; similarly, the coordinates of emitters A, B, C and D are <0, 
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-w, h>, <0, w, h>, <0, -w, -h> and <0, w, -h>, respectively. The offset of the frame of 
plate 2 is <x, y, z, a, fi, y>, with respect to the frame of plate 1 (in the case of two 
plates facing each other, the offset is <x, 0, 0, tt, 0, 0>), Tis the transform matrix from 
plate 1, 90, to plate 2, 92, obtained by the offset, and R is the rotation matrix of T. 
The norm of plate 1 is , <1, 0, 0> and the norm of the plate 2 in plate 1 coordinates is 
R<\, 0, 0>. Then <x e , y e , z e > is made the coordinate of the emitter in its own frame 
and <x r , y r , z r > is made the coordinate of the receiver of the opposing plate in its own 
frame. Under this scenario there are two cases: 

(i) The emitter is on plate 1 and the receiver is on plate 2: the position of the 
emitter is o = <x e , y e , z e > and the position of the receiver is q = Tp where p = <x r , y r , 
Zf> and q = <x r , y r , z r >. 

(ii) The emitter is on plate 2 and the receiver is on plate 1 : the position of the 
receiver is o = <x r , y r , z r >, and the position of the emitter is q = Tp where p = <x e , 
y e , z e > and q = <x e , y e , z e > 

[0036] Given two points in space, o and q, and the norms of their plates, n 0 and rig, 
the distance between them is \q-o\, the angle at o is arccos(n 0 »(q-o)f\q-o\) and the angle 
at q is arccos(riq»(q-o))/\q-o\. Therefore, the emitter and receiver angles as well as the 
distance between the receiver and the emitter can be obtained for each of the 16 pairs of 
emitters and receivers. Given the IR intensity model obtained in the previous section, a 
model from each six DOF offset between two plates to 16 readings of intensities is 
defined as: 

k =fi(x, y, z, a, j3, Y ) for/= 1 to 16. 
4.1. Absolute Six DOF Position Sensing 

[0037] Theoretically, the problem of six DOF offset estimation is a problem of data 
fitting, i.e., solving <x, y, z, a, j3, y> given sixteen data readings. In particular, letting Rf, 
i = I.. 16, by the sixteen readings and letting E be an energy function to be minimized, 
then: 

which transforms to six equations: 
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where is x, j/, z, a; j8 and y: 

[0038] This set of equations can then be solved using Newton's method. In this 
embodiment Singular Value Decomposition (SVD) was used for solving linear equations 
at each Newton step. The use of SVD greatly reduces the risk of reaching a singularity 
that is common in problems involving the inverse of matrices. It also achieves an 
improved result in both under-constrained (minimum change) and over-constrained 
(minimum error) situations. 
4.2. Relative Six DOF Offset Sensing 

[0039] In the close loop control of the docking process, it is not necessary to have 
absolute six DOF position sensing, as long as the system (1) is able to tell the direction 
of the offset and (2) is able to tell the offset between the plates is small enough to be 
within a working range. The first requirement (1) is used to guide the motion of the 
plates, and the second requirement (2) is used to trigger the mechanical connections to 
operate at the right time. 

[0040] A centering method has been developed based on the idea of signal 
"balancing" when the plates are centered and facing each other (see FIGURE 7). Let Xij 
represent a reading where Xis the emitter ID (A, B, C, or D), i is the receiver ID (1 or 2) 
andy is the plate ID (1 or 2) and let _ represent the case that holds for both plate 1 and 2. 
When two plates are centered and facing each other, a set of equations are developed: 

A1_=B1_, A2_=B2_, C1_=D1_, C2_=D2_. 
[004 1 ] In practice, even when the two plates are exactly centered, the equations 
may not hold because of noise and slight variations when mechanically assembling 
the plates. The difference, however, can be used as a guideline for a relative offset. 
For example, (A1_-B1 J+(A2_-B2J+(C1_-D1 J+(C2_-D2J gives offset in the Y 
direction, while (A 1 _-C2 J)+(A2_-C 1 )+(B 1 _-D2_)+(B2_-D 1 _) gives relative offset in 
the Z direction. This method has been used to successfully dock two plates in a plane, 
i.e., a special case with three DOF offset. 
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[0042] To follow this path further, we discovered five "balancing" equations, each 
of which corresponds to an invariant with respect to all but one variable of the six 
DOF offset: 

1. Y variant (x, z, a, (3, y invariant): B12+D22+B1 1+D21 = 
A12+C22+A11+C21, B22+D12+B21+D11 = A22+C12+A21+C1 1. 

2. Z variant (x, y, a, p, y invariant): A12+B12+D21+C21 = 
C22+D22+B11+A11, A22+B22+D11+C11 = C12+D12+B21+A21. 

3 . a variant (x, y, z, p, y invariant) : B 1 2+A 1 1 +D22+C2 1 = A 1 2+B 1 1 + 
C22+D21, D12+C1 1+ B22+A21 = C12+D1 1+ A22+B21. 

4. p variant (x, y, z, a, y invariant): A12+B1 1+B12+A1 1 = C22+D21+D22+C21, 
A22+B21+B22+A21 = C12+D11+D12+C11. 

5 . y variant (x, y, z, a, p invariant) : B 1 2+C22+B 1 1 +C2 1 = A 1 2+D22+A 1 1 +D2 1 , 
A22+D 1 2+A2 1 +D 1 1 = B22+C 12 + B21+C11. 

[0043] A minimization method was then developed which can be used for any of the 
equations. For example, for equation A =B, an energy function is defined as: 

E = ±(A-B) 2 . 

[0044] Note that this energy function does not have the explicit IR model as the one 

used for the absolute position sensing. The goal of centering is to move to the direction 

where the energy function can be minimized. In order to minimize E, we calculate: 

8E . t n ..dA dfl d 2 E ,dA dB^ 2 ..... _ 

— = (A- B)( ) , —t— » ( ) , in which, p is y, z, a, J3 or y. 

dp dp dp dp dp dp 

8E 8 2 E 

[0045] By letting Ap = / — - — , obtained is the direction of the offset movement 

dp dp 

to minimize the energy function defined by the equation. Because each equation 
corresponds to exactly one variant, that equation is used to calculate the offset of that 
variant, i.e., equation 1 to calculate y, equation 2 to calculate z, equation 3 to calculate 
a, equation 4 to calculate p and equation 5 to calculate y. To calculate x, we use energy 

function E = ^^Xy , based on the fact that all the readings go to zero when x 
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approaches 0 in centered position. For simplicity, assuming the plates are centered, we 

. , dE ,d 2 E , dE ~ dX. d 2 E ^ dX 

have Ax = /— — , where — = > (X„ — -) and —r— « > ( — J -Y . 

[0046] It is to be understood that the relative method relies on the symmetry in the 
emitter/receiver positions, while the absolute method does not. 

5. Experimental Results for the First Embodiment 

[0047] An experimental setup for measuring six DOF offset has been developed. The 
setup includes two PolyBot modules each of which has an IR plate and one module for 
calculating the offset. The three modules communicate via CANbus and each contains 
an MPC555. The outputs of the six DOF offset are sent from the computing MPC555 
to a CANalyzer, which is a CAN interface program running on a PC. The experiments 
are done basically by fixing one IR module and moving the other IR module in space. 
[0048] The solution described was originally intended for use with modular 
reconfigurable robots. The intent is to provide a simple and easy system that the robot 
can use to accomplish closed-loop docking of two serial chains of modules. It is assumed 
that using open-loop control, the robot will achieve the goal of docking with errors in up 
to six DOF. The absolute method does not work as well as expected, the relative method 
does successfully dock two plates in demo. 

6. Introduction to a Second Face Plate Embodiment 

[0049] Turning to FIGURE 8, a second embodiment of the connection/face plates 
100a, lOOn of modules 14a and 14n (FIGURES 1 and 2) are illustrated. In this 
embodiment, the face plates are designed with emitter/receiver pairs. For example, on 
face plate 100a, emitter 102a is paired with receiver 104a at an area near the edge of the 
face plate. Remaining emitters 102b, 102c and 104d are paired with receivers 104b, 104c 
and 104d, respectively. These emitters and receivers, similar to the discussion in 
connection with FIGURE 3, may also be designed with COTS LEDs and COTS IR 
receivers or other appropriate devices as mentioned in connection with the first 
embodiment. With continuing attention to FIGURE 8, face plate lOOn is configured to 
be identical to face plate 100a. Therefore emitters 106a, 106b, 106c and 106d are paired 
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in a positional relationship with receivers 108a, 108b, 108c and 108d at positions 
identical to the position on face plate 100a. 

[0050] Face plate 1 00a also has electrical connections and mechanical connections, 
which are aligned identically with electrical connections and mechanical connections of 
face plate lOOn. 

[005 1 ] In an operational embodiment of a system implementing face plates 1 00a and 
lOOn of FIGURE 8, each of the 8 emitters are lit in sequence and an analog reading is 
taken from all four of the opposing receivers on the opposite face plate. The intensity 
distribution of the emitters and the sensitivity distribution of the receivers permit for an 
estimation of the angle and distance between these components. This design permits 
obtaining of positional offset information in any of six degrees of error (x, y, z, pitch, roll 
and yaw) within a predetermined range. In this embodiment it is not necessary to have 
the receivers on the face plate emitting signals to also receive those signals. 
[0052] A theoretical framework is also established for this design using the least 
squares minimization. The theoretical framework is general and applies to other 
configurations of emitter and receiver parts and positioning. When the offset between 
the two plates is out of the range, or when the model of IR emitter-receiver loses the 
sensitivity, a relative method based purely on the geometry positions of the emitters and 
receivers can be used. 

[0053] The design of FIGURE 8 has the property that when the two faces are closer, 
the intensities received by receivers from signals generated by corresponding emitters 
increase. 

[0054] As in the previous design, the electrical design ensures that each receiver 
detects and samples the intensity from each emitter on the opposite plate at a distinct 
time. However, data only from one module may be used, rather than from both sides in 
the first embodiment. In order to do that, each of the four emitters is lit in sequence and 
an analog reading is taken from each of the four opposing receivers. Readings are also 
taken in between the times when the IR LEDs are emitting to measure the ambient IR. 
At the end of a time period, each of the receivers will have four readings from their 
opposing emitters, and four ambient readings totaling 32 measurements. The ambient 
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IR readings are subtracted from the preceding sample to make the system more robust. 
Following the subtraction, 16 pieces of date are available for use. 

7. IR Intensity Model of the Second Face Plate Embodiment 

[0055] The theory behind this design is based on the fact that the intensity detected 
by a receiver is a function of the distance and/or angle between the emitter and receiver, 
i.e., / = f(e, r, d) where / is the intensity reading, e and r are emitter and receiver angles, 
respectively, and d is the distance between the emitter and the receiver. An accurate 
model can be obtained by model fitting for given emitters and detectors. Different from 
the embodiment of FIGURE 3, this implementation uses an IR emitter-receiver pair that 
is not so sensitive to emitter and receiver angles, but very sensitive to distance, the 
emitter intensity and the receiver sensitivity are also adjusted so that the distance curve 
has a wide sensitivity range. Using model f(d) = A/d 2 +0, O is set to be 35, A is obtained 
via a least square curve fitting, which is 217. Figure 9 shows the actual curve 1 10 and 
model curve 1 12 of the IR emitter-receiver pair used for the setting. 

8. Six Dimensional Offset Estimation Methods for the Second Embodiment 

[0056] For each face plate, a frame may be attached as shown in Figure 1 0 (in this 
case Plate 1(114) and Plate 2 (1 1 6) are facing each other). Given an offset between the 
two plates, the spatial relationship between each pair of emitter and receiver is 
determined. 

[0057] Letting S be the shorter distance from the emitter or receiver to the center of 
the plate, and L the longer distance to the center of the plate. The coordinate of four 
receivers are <0, L, -S>, <0, S, L>, <0, -L, S> and <0, -S, -L>, respectively, and the 
coordinates of the four emitters are <0, -L, -S>, <0, -S, L>, <0, L, S> and <0, S, -L>, 
respectively. Let <x, y z, a, f3, y> be the offset of the frame of plate 2 with respect to the 
frame of plate 1 (in the case of two plates facing each other, the offset is <x, 0, 0, 7t, 0, 
0>) and let Tbe the transform matrix from plate 1 to plate 2 obtained by the offset, and 
R be the rotation matrix of T. The norm of plate 1 is <1 , 0, 0> and the norm of the plate 
2 in plate 1 coordinates is R<1, 0, 0>. Let <x r , y r , z r > be the coordinate of the receiver 
in its own frame in plate 1 and <x e , y e , z e > be the coordinate of the emitter of plate 2 in 
its own frame, the position of the receiver is o = <x r , y r , z r >, and the position of the 
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emitter is q = Tp where p = <x e , y e , z e >. 

[0058] Given two points in space, o and q, and the norms of their plates, n 0 and n q , 
the distance between them is \q-o\, the angle at o is arccos(n 0 *(q-o)/\q-o\) and the angle 
at q is arccos(n q »(o-q)/\q-o\). Therefore, the emitter and receiver angles as well as the 
distance between the receiver and the emitter can be obtained for each of the sixteen pairs 
of emitters and receivers. Given the IR intensity model we obtained in the previous 
section, we get a model from each six DOF offset between two plates to 16 readings of 
intensities, i.e., I ; = f ; (x, y, z, a, p\ y) for 1=1 to 16. 

8.1. Absolute Six DOF Position Sensing 

[0059] Theoretically, the problem of six DOF offset estimation becomes a 
problem of data fitting, i.e., solving <x, y, z, a, J3, y> given sixteen data readings. In 
particular, let Rj, i =1 .. 1 6, be the sixteen readings and let E be an energy function to be 
minimized, e = ~ J]j 6 (R, - / ; ( x , y , z , a, j3, y)) 2 which transforms to six equations: 

= 5T ( R , ~ f, ( x > y> z > a > r)) ^ = °> where P is x, y, z, a, j3, and y. 

This set of equations can then be solved using Newton's method, by using Singular Value 
Decomposition (SVD) for solving linear equations at each Newton step. The use of SVD 
greatly reduces the risk of reaching a singularity that is very common in problems 
involving the inverse of matrices. It also achieves a better result in both under- 
(minimum change) and over-constrained (minimum error) situations. 

8.2. Relative Six DOF Offset Sensing 

[0060] In close loop control of the docking process, it is not necessary to have 
absolute six DOF position sensing, as long as (1) it can tell the direction of the offset and 
(2) it can tell if the offset between the plates are small enough; (1) is used to guide the 
motion of the plates and (2) is to trigger the latches in the plates to open and close at the 
right time. 

[0061] A centering method was therefore developed based on the idea of signal 
"balancing" when the plates are centered and facing each other. Let Vij represent a 
reading where i is the emitter ID and j is the receiver ID. When two plates are centered 
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and facing each other, we have a set of equations, e.g., VI 1=V33, V22=V44. In practice, 
even when the two plates are exactly centered, the equations may not hold because of 
noise and slight variations when mechanically assembling the plates. The difference, 
however, can be used as a guideline for a relative offset. For example, (V32-V12)+(V34- 
V14)+(V21-V23)+(V41-V43) gives offset in Y direction, while (V43-V23)+(V41- 
V21)+(V32-V34)+(V12-V14) gives relative offset in Z direction. This method has been 
used to successfully dock two plates in a plane, i.e., a special case with three DOF offset, 
even in the previous design. 

[0062] To follow this path further, five "balancing" equations were developed, each 
of which corresponds to an invariant with respect to all but one of six DOF offset: 
Y Variant (x, z, a, p, y invariant): V32 + V21 + V34 + V41= V23 + V12 + V43 + V14. 
Z Variant (x,.y, a, p, y invariant): V43 + V32 + V41 + V12 = V23 + V34 + V21 + V14. 
a Variant (x, y, z, p, y invariant): VI 1=V33. 
P Variant (x, y, z, a, y invariant): V22=V44. 

y Variant (x, y, z, a, p invariant): V41 + V12 + V23 + V34 = V21 + V32 + V43 + V14. 
[0063] A minimization method was then developed which can be used for any of the 
equations. For example, for equation A =B, an energy function is defined as: 

E = ±(A-B) 2 . 

[0064] Note that this energy function does not have the explicit IR model as the one 
used for the absolute position sensing. The goal of centering is to move to the direction 
where the energy function can be minimized. In order to minimize E, we calculate: 

— = (A-B)(- — — ), — — « (— — — ) , in which, p is y, z, a, poxy. 

op dp dp dp dp dp 

[0065] By letting Ap = -~j^L } obtained is the direction of the offset movement 
op d p 

to minimize the energy function defined by the equation. Because each equation 
corresponding to exactly one variant, that equation is used to calculate the offset of that 
variant, i.e., equation 1 to calculate y, equation 2 to calculate z, equation 3 to calculate 
a, equation 4 to calculate p and equation 5 to calculate y. To calculate x, we use energy 
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function E = -- J] V t f , based on the fact that all the four readings go to maximum 

1 '=1,2,3,4 

when x approaches 0 in centered position. For simplicity, assuming the plates are 
centered, we have a* = -|*/^, where^ = - Y (Vii?™) and 

V ( dVii ^ 

[0066] In the case where the IR curve is flat (distance close to 0 or larger than a 
maximum distance the system will work; in one embodiment 5 inches), direct estimation 
is used, i.e., y = V32 + V21 + V34 + V41 - V23 + V12 + V43 + V14, z = V43 + V32 + 
V41 + V12 - V23 + V34 + V21 + V14, a = VI 1 - V33, £ = V22-V44, y= V21 + V32 
+ V43 + V14 - V41 + V12 + V23 + V34. 

[0067] The threshold to decide the direction depends on the noise level. For x, we 
can get an estimation by calculating V = (VI l+V22+V33+V44)/4 and find the 
corresponding distance value from the IR model curve. 
9. Experimental Results for the Plate Embodiment 

[0068] Experiments using both absolute and relative position sensing methods of the 
second embodiment were undertaken. In the current embodiment using an embedded 
chip, MPC555 with 20MHz, it took about 2.5 s to obtain an absolute position from an 
unknown start; it took 0.25s if the start point is not far from the actual position. The 
relative method is very efficient, it took less than 1 tick (60Hz). Relative and absolute 
position sensing may be used. The relative method is used for two purposes: first, it is 
a reliable method when the distance is out of range and secondly, it is used as a 
correctness or bound check for the absolute method, since the numerical algorithm used 
in the absolute method can go to local minimum from time to time. 
[0069] This second embodiment illustrates an improved IR curve to reduce the 
saturation range and extend sensitivity in the unsaturated range, so that six DOF absolute 
positioning is obtained. The positions of IR emitters and receivers have also been 
arranged so that the receivers still measure signals when the two plates are docked 
without loss of sensitivity. Both improvements increase the operation of obtaining the 
absolute six DOF offset for the second embodiment. 
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[0070] Turning to FIGURE 11, illustrated is a block diagram of one potential 
implementation for the triggering of emitters, the reception of the emitter signals and the 
processing of the received data signals in accordance with the present invention. 
Particularly, trigger signal generator/clock 120 is in operational connection with emitters 
122a-122d of emitter/receiver pairs 122a-122d, 123a-123d, and generates emitter control 
signals for activating the emitters 122a-122d of the face plate. These signals would be 
received by receivers on another similarity arranged (not shown faceplate). In the same 
manner the other face plate (not shown) will emit signals which are received by receivers 
123a-123d. A converter 124 in operational connection with receivers 123a-123d obtain 
the analog signals and converts the signals into a digital form. The converter then 
provides the digital data to a processing system 128 for generation of offset instructions. 
It is to be appreciated, while the face plate implementing emitter/receiver pairs is shown 
(FIGURE 8), the described system may also be implemented for use with the face plate 
embodiment of FIGURE 3 as well as other arrangements. Also, while FIGURE 1 1 shows 
the system with wire connections, it is to be understood that the present applications are 
also capable of being formed in other arrangements such as wireless formats. 
[007 1 ] Typically, each module has one microprocessor (such as a Motorola MPC5 55 
Power PC), where the IR data is converted. If data from both sides is required, data from 
one side may send this data to the other module, or both modules may send data to a 
master module, where computations take place. In processor a TPU3 (time processing 
unit) generates the trigger and emitter control signals. The trigger signal is fed back into 
a QADC64 (queued analog to digital converter) external trigger input line to obtain the 
readings from the IR detectors. A list of conversions is initially programmed into the 
analog digital queue and a single interrupt is generated at the end of each complete 
period. The interrupt service routine is responsible for subtracting the ambient 
measurements and sending the data to a master computation thread. The microprocessor 
may send this data over a CANbus network to a master, which may also be an MPC555 
micro-controller, wherein an algorithm for finding the six degree of freedom position 
offset is implemented. 
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10. Conclusion 

[0072] In the foregoing discussion, it is to be appreciated that the disclosed 
embodiments provide a symmetry between face plates. This symmetry is desirable to 
permit alignment and/or docking between the modules. Thus, these modules are not only 
able to align or link-up but they are capable of accomplishing this even if the modules 
have been rotated with respect to each other. 

[0073] The disclosed concepts provide a low cost six degree of freedom offset 
sensing capability between two components such as the described modules. However, 
it is to be understood that components and modules should be construed in their broadest 
sense to mean any elements which are to be aligned. Further, while the discussion 
focussed on use in connection with modular, self-configurable robots, other environments 
may also take advantage of the low-cost measurement and actuation concepts of 
embodiments of the present invention, enabling applications beyond robotic docking. 
Such systems may include, for example, use with digital cable TV set-top box navigation, 
which is presently done with arrow buttons to move the highlighted selection on the 
screen. Implementing this concept, one could use a remote control as a pointer. The set- 
top box would measure the relative position of the remote control in the same manner as 
a mouse on a computer system is presently used today. In this use, one embodiment 
would place four emitters on the handheld pointer and four receivers on the TV box. 
Such a system may be represented by FIGURE 1 1 to illustrate the handheld pointer, 
where the receivers are removed. Similarly, FIGURE 1 1 may represent a portion of the 
TV box where the four emitters are removed and four receivers are positioned to receive 
the transmitter signals. 

[0074] Other implementations may include cars that park themselves, toys, jacks on 
the back of the computer or stereo that move into position as they are being plugged in, 
a gas nozzle that finds a car's tank opening, or robot appliance that automatically dock 
or recharge, fluid or supply interfaces in the home, office or business, among other uses. 
[0075] The foregoing is considered as only illustrative of the principles of the 
invention. Further, since numerous modifications and changes will readily occur to those 
skilled in the art, it is not desired to limit the invention to the exact construction operation 



18 



as shown and described, and accordingly all suitable modifications and equivalents may 
be considered as falling within the scope of the invention. 



19 



